#include "udp_mavlink.h"
#include "QDebug.h"

#include "qudpsocket.h"
#include "../Mavlink_V1.0/all/mavlink.h"
#include "../Mavlink_V1.0/ardupilotmega/mavlink.h"
#include "qthread.h"

#include "../window/mainwindow.h"

udp_mavlink * User_Send;

extern MainWindow *uwindow;

udp_mavlink::udp_mavlink(const QString &udpAdd, const QString &udpPort, const QString &udpPortlocal) {
    server = new QUdpSocket;
    udp_add = udpAdd;
    udp_port_remote = udpPort;
    udp_port_local = udpPortlocal;

    while (true) {
        if (!server->bind(QHostAddress::Any, udp_port_local.toUShort())) {
            qDebug() << "Bind Failed";
        } else {
            qDebug() << "Bind Success";
            break;
        }
        QThread::msleep(100);
    }
    //添加udp读取连接函数
    if (!connect(server, SIGNAL(readyRead()), this, SLOT(messageReceive()))) {
        qDebug() << "Failed to connect readyRead signal to messageReceive slot";
    } else {
        qDebug() << "Successfully connected readyRead signal to messageReceive slot";
    }
}

udp_mavlink::~udp_mavlink() {
}

/**
 * @brief 通过udp发送mavlink数据真值
 * @param hexDatah mavlink数据
 * @param len mavlink数据长度
 */
void udp_mavlink::mavlink_send_udp(char *hexDatah, char len) {
    // 将 hexDatah 从 char* 转换为 QByteArray
    QByteArray data(hexDatah, len);

    // 将 udp_add 从 QString 转换为 QHostAddress
    QHostAddress hostAddress(udp_add);

    // 将 udp_port 从 QString 转换为 quint16
    quint16 remote_port = udp_port_remote.toUShort();

    server->writeDatagram(data, hostAddress, remote_port);
}

/**
 * @brief 接收udp数据后的处理函数 slot
 */
void udp_mavlink::messageReceive() {
    QByteArray datagram;

    while (server->hasPendingDatagrams()) {
        datagram.resize(server->pendingDatagramSize());
        server->readDatagram(datagram.data(), datagram.size());
        // emit message_share(&datagram,datagram.size());
        //数据包转为mavlink缓存
        uint8_t buff[datagram.size()];
        memcpy(buff, datagram.data(), datagram.size());
        //解析mavlink消息
        mavlink_msg_analysis(buff, datagram.size());
    }
}

/**
 * @brief 解析mavlink消息
 * @param buff_add 收到的mavlink消息
 * @param len 收到的mavlink消息长度
 */
void udp_mavlink::mavlink_msg_analysis(uint8_t *buff_add, uint8_t len) {
    mavlink_message_t msg; //mavlink消息
    mavlink_status_t status; //mavlink状态
    //我相信udp的速度很快 , 我不用担心队列的问题
    for (int i = 0; i < len; i++) {
        if (mavlink_parse_char(MAVLINK_COMM_0, buff_add[i], &msg, &status) == 1) {
            //判断消息类型
            switch (msg.msgid) {
                case MAVLINK_MSG_ID_HEARTBEAT: {
                    //处理心跳包
                    mavlink_heartbeat_t heartbeat;
                    mavlink_msg_heartbeat_decode(&msg, &heartbeat);
                    switch (heartbeat.custom_mode) {
                        case MainWindow::wall_mode: {
                            robo_info.clean_mode = "墙壁模式";
                            break;
                        }
                        case MainWindow::fool_mode: {
                            robo_info.clean_mode = "池底模式";
                            break;
                        }
                        case MainWindow::carmera: {
                            robo_info.clean_mode = "相机模式";
                            break;
                        }
                        case MainWindow::rc_ctrl: {
                            robo_info.clean_mode = "遥控模式(测试专属)";
                            break;
                        }
                        default:
                            break;
                    }

                    switch (heartbeat.system_status) {
                        case FORWARDING: {
                            robo_info.now_state = "前进";
                            break;
                        }
                        case BACKING: {
                            robo_info.now_state = "后退";
                            break;
                        }
                        case Probe: {
                            robo_info.now_state = "探索";
                            break;
                        }
                        case Turn_left_90: {
                            robo_info.now_state = "左转90度";
                            break;
                        }
                        case Turn_right_90: {
                            robo_info.now_state = "右转90度";
                            break;
                        }
                        case Turn_180: {
                            robo_info.now_state = "旋转180度";
                            break;
                        }
                        case Find: {
                            robo_info.now_state = "寻找对齐";
                            break;
                        }
                        case PENDING: {
                            robo_info.now_state = "挂起";
                            break;
                        }
                        case RAISING: {
                            robo_info.now_state = "升起";
                            break;
                        }
                        case CLIMBING: {
                            robo_info.now_state = "爬墙";
                            break;
                        }
                        case LATERAL: {
                            robo_info.now_state = "横移";
                            break;
                        }
                        case DIVING: {
                            robo_info.now_state = "潜水";
                            break;
                        }
                        case BYPASSING: {
                            robo_info.now_state = "绕过";
                            break;
                        }
                        case ALIGNING: {
                            robo_info.now_state = "对齐";
                            break;
                        }
                        case TURNING: {
                            robo_info.now_state = "转弯";
                            break;
                        }
                        case PAUSE: {
                            robo_info.now_state = "暂停";
                            break;
                        }
                        case INTERRUPT: {
                            robo_info.now_state = "中断";
                            break;
                        }
                        case CHECK_MOTORS: {
                            robo_info.now_state = "检查电机";
                            break;
                        }
                        case CHECK_PUMPS: {
                            robo_info.now_state = "检查泵";
                        }
                        case INIT: {
                            robo_info.now_state = "初始化";
                            break;
                        }
                        default: {
                            robo_info.now_state = "停止";
                            break;
                        }
                    }
                    break;
                }
                case MAVLINK_MSG_ID_SYS_STATUS: {
                    mavlink_sys_status_t sys_status;
                    mavlink_msg_sys_status_decode(&msg, &sys_status);
                    robo_info.battery_volt = QString::number(sys_status.voltage_battery / 1000.0f, 'f', 2);
                    if (sys_status.current_battery > 0) {
                        robo_info.battery_current = "充电中:" + QString::number(sys_status.current_battery, 'f', 1);
                    } else {
                        robo_info.battery_current = QString::number(-sys_status.current_battery, 'f', 1);
                    }
                    robo_info.battery_percent = QString::number(sys_status.battery_remaining, 'd', 1);
                    robo_info.battery_power = QString::number((-sys_status.current_battery/1000.0f) * (sys_status.voltage_battery / 1000.0f),'f',3);
                    break;
                }
                case MAVLINK_MSG_ID_SCALED_IMU: {
                    mavlink_scaled_imu_t imu;
                    mavlink_msg_scaled_imu_decode(&msg, &imu);
                    robo_info.acc_last.x = robo_info.acc.x;
                    robo_info.acc_last.y = robo_info.acc.y;
                    robo_info.acc_last.z = robo_info.acc.z;

                    robo_info.acc.x = imu.xacc;
                    robo_info.acc.y = imu.yacc;
                    robo_info.acc.z = imu.zacc;
                    robo_info.gyro.x = imu.xgyro;
                    robo_info.gyro.y = imu.ygyro;
                    robo_info.gyro.z = imu.zgyro;
                    robo_info.mag.x = imu.xmag;
                    robo_info.mag.y = imu.ymag;
                    robo_info.mag.z = imu.zmag;
                    break;
                }
                case MAVLINK_MSG_ID_SCALED_PRESSURE: {
                    //msg_id: 29
                    mavlink_scaled_pressure_t pressure;
                    mavlink_msg_scaled_pressure_decode(&msg, &pressure);
                    robo_info.time_tick = pressure.time_boot_ms; //ms时间戳
                    break;
                }

                case MAVLINK_MSG_ID_ATTITUDE: {
                    //msg_id: 30
                    mavlink_attitude_t angle;
                    mavlink_msg_attitude_decode(&msg, &angle);
                    //处理姿态信息
                    robo_info.euler.x = angle.roll;
                    robo_info.euler.y = angle.pitch;
                    robo_info.euler.z = angle.yaw;
                    break;
                }
                case MAVLINK_MSG_ID_ATTITUDE_QUATERNION: {
                    mavlink_attitude_quaternion_t quaternion;
                    mavlink_msg_attitude_quaternion_decode(&msg, &quaternion);
                    robo_info.quat[0] = quaternion.q1;
                    robo_info.quat[1] = quaternion.q2;
                    robo_info.quat[2] = quaternion.q3;
                    robo_info.quat[3] = quaternion.q4;
                    break;
                }
                case MAVLINK_MSG_ID_TIMESYNC: {
                    //msg_id: 111

                    break;
                }
                case MAVLINK_MSG_ID_AUTOPILOT_VERSION: {
                    mavlink_autopilot_version_t autopilot_version;
                    mavlink_msg_autopilot_version_decode(&msg, &autopilot_version);
                    //将版本号转为字符串

                    QString major_version = QString::number((autopilot_version.flight_sw_version&0xff000000)>>24,'h',0);
                    QString minor_version = QString::number((autopilot_version.flight_sw_version&0x00ff0000)>>16,'h',0);
                    QString patch_version = QString::number((autopilot_version.flight_sw_version&0x0000ff00)>>8,'h',0);
                    QString build_version = QString::number(autopilot_version.flight_sw_version&0x000000ff,'h',0);
                    robo_info.fw_version = major_version + "." + minor_version + "." + patch_version + "." + build_version;

                    robo_info.hw_version = QString::number(autopilot_version.middleware_sw_version, 16);
                    robo_info.bt_version = QString::number(autopilot_version.os_sw_version, 16);
                    break;
                }
                case MAVLINK_MSG_ID_DATA16: {
                    mavlink_data16_t data16;
                    mavlink_msg_data16_decode(&msg, &data16);
                    // 转换为字符串
                    switch (data16.type) {
                        case 1: {
                            //驱动电机转速;
                            memcpy(&robo_info.moto_rmp_left, &data16.data[0], 2);
                            memcpy(&robo_info.moto_rmp_right, &data16.data[2], 2);
                            memcpy(&robo_info.pump_rmp_left, &data16.data[4], 2);
                            memcpy(&robo_info.pump_rmp_right, &data16.data[6], 2);
                            break;
                        }
                        default: {
                            break;
                        }
                    }
                    break;
                }
                case MAVLINK_MSG_ID_DATA32: {
                    mavlink_data32_t mavlink_data32;
                    mavlink_msg_data32_decode(&msg, &mavlink_data32);
                    // 转换为字符串
                    switch (mavlink_data32.type) {
                        case 0: {
                            //工况消息
                            break;
                        }
                        case 1: {
                            //遇水
                            if (mavlink_data32.data[0] == 1) {
                                robo_info.has_water_in = "有水";
                            } else {
                                robo_info.has_water_in = "无水";
                            }

                            if (mavlink_data32.data[1] == 1) {
                                robo_info.has_water_left = "有水";
                            } else {
                                robo_info.has_water_left = "无水";
                            }

                            if (mavlink_data32.data[2] == 1) {
                                robo_info.has_water_right = "有水";
                            } else {
                                robo_info.has_water_right = "无水";
                            }
                            break;
                        }
                        case 2: {
                            memcpy(&robo_info.flow_x, &mavlink_data32.data[0], 2);
                            memcpy(&robo_info.flow_y, &mavlink_data32.data[2], 2);
                            robo_info.flow_x_sum  += robo_info.flow_x; //积分一下
                            robo_info.flow_y_sum  += robo_info.flow_y; //积分一下
                            //光流
                            break;
                        }
                        case 3: {
                            //lidar data copy
                            memcpy(&robo_info.lidar[0], &mavlink_data32.data[0], 2);
                            memcpy(&robo_info.lidar[1], &mavlink_data32.data[2], 2);
                            memcpy(&robo_info.lidar[2], &mavlink_data32.data[4], 2);
                            memcpy(&robo_info.lidar[3], &mavlink_data32.data[6], 2);

                            break;
                        }
                        case 4: {
                            memcpy(&robo_info.high_in, &mavlink_data32.data[0], 4); //高度传感器 float值
                            break;
                        }
                        default:
                            break;
                    }
                    break;
                }
                case MAVLINK_MSG_ID_DATA64: {
                    mavlink_data64_t data64;
                    mavlink_msg_data64_decode(&msg, &data64);
                    if (data64.type == 0) {
                        QByteArray data_buffer;
                        data_buffer.append(reinterpret_cast<char *>(&data64.data), data64.len);
                        emit message_share(&data_buffer, data64.len);
                        //是自定义消息
                    }
                    break;
                }
                default: {
                    break;
                }
            }
            break;
        }
    }
}
